#ifndef GAMEENGINE_GEOMETRY_COORDINATESYSTEMS_TRANSFORMER3D_ORIENTATION_HPP
#define GAMEENGINE_GEOMETRY_COORDINATESYSTEMS_TRANSFORMER3D_ORIENTATION_HPP

#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif

//=================================================================================
// Implementation of the Transformer3dOrientation class

//! Default constructor
/*!
 @note Class T should have a default constructor
 */
template <class T>
Transformer3dOrientation<T>::Transformer3dOrientation() {
	// Setup as identity
	matrix_[0][0] = (T)1;
	matrix_[0][1] = (T)0;
	matrix_[0][2] = (T)0;
	matrix_[1][0] = (T)0;
	matrix_[1][1] = (T)1;
	matrix_[1][2] = (T)0;
	matrix_[2][0] = (T)0;
	matrix_[2][1] = (T)0;
	matrix_[2][2] = (T)1;
}

//! Constructor
/*!
 *  Builds a 3d transformer that converts orientations from1, from2 and from3 into orientations to1, to2 and to3
 *
 @param from1 The orientation to transform into to1 in the destination coordinate system
 @param from2 The orientation to transform into to2 in the destination coordinate system
 @param from3 The orientation to transform into to3 in the destination coordinate system
 @param to1 The orientation in which from1 should be transformed in the destination coordinate system
 @param to2 The orientation in which from2 should be transformed in the destination coordinate system
 @param to3 The orientation in which from3 should be transformed in the destination coordinate system
 @sa Function set_transformation is called by this constructor
 @note Class T should have a default constructor
 */
template <class T>
Transformer3dOrientation<T>::Transformer3dOrientation(const Vector3d<T>& from1, const Vector3d<T>& from2, const Vector3d<T>& from3,
		const Vector3d<T>& to1, const Vector3d<T>& to2, const Vector3d<T>& to3)
{
	set_transformation(from1, from2, from3, to1, to2, to3);
}

//! Copy constructor
/*!
 @param rhs The transformer to copy
 @note Class T should have a default constructor
 @note Class T should have an assignment operator
 */
template <class T>
Transformer3dOrientation<T>::Transformer3dOrientation(const Transformer3dOrientation& rhs) {
	// Copy the transformation matrix
	for ( int k = 0; k < 3; ++k ) {
		for ( int l = 0; l < 3; ++l ) {
			matrix_[k][l] = rhs.matrix_[k][l];
		}
	}
}

//! Destructor
/*!
 @note The destructor of class T should be public
 */
template <class T>
Transformer3dOrientation<T>::~Transformer3dOrientation() {}

//! Assignment operator
/*!
 @param rhs The transformer to copy
 @note Class T should have an assignment operator
 */
template <class T>
Transformer3dOrientation<T>& Transformer3dOrientation<T>::operator=(const Transformer3dOrientation& rhs) {
	if ( &rhs != this ) {
		// Copy the transformation matrix
		for ( int k = 0; k < 3; ++k ) {
			for ( int l = 0; l < 3; ++l ) {
				matrix_[k][l] = rhs.matrix_[k][l];
			}
		}
	}
	return *this;
}

//! Setup a three-dimensional transformation
/*!
 @param from1 The orientation to transform into to1 in the destination coordinate system
 @param from2 The orientation to transform into to2 in the destination coordinate system
 @param from3 The orientation to transform into to3 in the destination coordinate system
 @param to1 The orientation in which from1 should be transformed in the destination coordinate system
 @param to2 The orientation in which from2 should be transformed in the destination coordinate system
 @param to3 The orientation in which from3 should be transformed in the destination coordinate system
 @return True if such a transformer exists. If no such transformation can be setup, the transformer is not modified.
 @note Class T should have an assignment operator
 */
template <class T>
bool Transformer3dOrientation<T>::set_transformation(const Vector3d<T>& from1, const Vector3d<T>& from2, const Vector3d<T>& from3,
		const Vector3d<T>& to1, const Vector3d<T>& to2, const Vector3d<T>& to3)
{
	// Compute denominator
	T den = den = from3.x()*from2.y()*from1.z() - from2.x()*from3.y()*from1.z() - from3.x()*from1.y()*from2.z()
		  + from1.x()*from3.y()*from2.z() + from2.x()*from1.y()*from3.z() - from1.x()*from2.y()*from3.z();
	if ( den == (T)0 ) return false;

	// Setup matrix
	matrix_[0][0] =  (to3.x()*from2.y()*from1.z() - to2.x()*from3.y()*from1.z() - to3.x()*from1.y()*from2.z()
		+ to1.x()*from3.y()*from2.z() + to2.x()*from1.y()*from3.z() - to1.x()*from2.y()*from3.z()) / den;
	matrix_[0][1] =  (from3.x()*to2.x()*from1.z() - from2.x()*to3.x()*from1.z() - from3.x()*to1.x()*from2.z()
		+ from1.x()*to3.x()*from2.z() + from2.x()*to1.x()*from3.z() - from1.x()*to2.x()*from3.z()) / den;
	matrix_[0][2] = -(from3.x()*to2.x()*from1.y() - from2.x()*to3.x()*from1.y() - from3.x()*to1.x()*from2.y()
		+ from1.x()*to3.x()*from2.y() + from2.x()*to1.x()*from3.y() - from1.x()*to2.x()*from3.y()) / den;
	matrix_[1][0] = -(from3.y()*to2.y()*from1.z() - from2.y()*to3.y()*from1.z() - from3.y()*to1.y()*from2.z()
		+ from1.y()*to3.y()*from2.z() + from2.y()*to1.y()*from3.z() - from1.y()*to2.y()*from3.z()) / den;
	matrix_[1][1] =  (from3.x()*to2.y()*from1.z() - from2.x()*to3.y()*from1.z() - from3.x()*to1.y()*from2.z()
		+ from1.x()*to3.y()*from2.z() + from2.x()*to1.y()*from3.z() - from1.x()*to2.y()*from3.z()) / den;
	matrix_[1][2] =  (from3.x()*from2.y()*to1.y() - from2.x()*from3.y()*to1.y() - from3.x()*from1.y()*to2.y()
		+ from1.x()*from3.y()*to2.y() + from2.x()*from1.y()*to3.y() - from1.x()*from2.y()*to3.y()) / den;
	matrix_[2][0] =  (from3.y()*from2.z()*to1.z() - from2.y()*from3.z()*to1.z() - from3.y()*from1.z()*to2.z()
		+ from1.y()*from3.z()*to2.z() + from2.y()*from1.z()*to3.z() - from1.y()*from2.z()*to3.z()) / den;
	matrix_[2][1] = -(from3.x()*from2.z()*to1.z() - from2.x()*from3.z()*to1.z() - from3.x()*from1.z()*to2.z()
		+ from1.x()*from3.z()*to2.z() + from2.x()*from1.z()*to3.z() - from1.x()*from2.z()*to3.z()) / den;
	matrix_[2][2] =  (from3.x()*from2.y()*to1.z() - from2.x()*from3.y()*to1.z() - from3.x()*from1.y()*to2.z()
		+ from1.x()*from3.y()*to2.z() + from2.x()*from1.y()*to3.z() - from1.x()*from2.y()*to3.z()) / den;
	return true;
}

//! Accessor to the first column of the transformation matrix (read only)
/*!
 @return The first column of the transformation matrix
 */
template <class T>
Vector3d<T> Transformer3dOrientation<T>::column1() const {
	return Vector3d<T>(matrix_[0][0], matrix_[1][0], matrix_[2][0]);
}

//! Accessor to the second column of the transformation matrix (read only)
/*!
 @return The second column of the transformation matrix
 */
template <class T>
Vector3d<T> Transformer3dOrientation<T>::column2() const {
	return Vector3d<T>(matrix_[0][1], matrix_[1][1], matrix_[2][1]);
}

//! Accessor to the third column of the transformation matrix (read only)
/*!
 @return The third column of the transformation matrix
 */
template <class T>
Vector3d<T> Transformer3dOrientation<T>::column3() const {
	return Vector3d<T>(matrix_[0][2], matrix_[1][2], matrix_[2][2]);
}

//! Accessor to the first row of the transformation matrix (read only)
/*!
 @return The first row of the transformation matrix
 */
template <class T>
Vector3d<T> Transformer3dOrientation<T>::row1() const {
	return Vector3d<T>(matrix_[0][0], matrix_[0][1], matrix_[0][2]);
}

//! Accessor to the second row of the transformation matrix (read only)
/*!
 @return The second row of the transformation matrix
 */
template <class T>
Vector3d<T> Transformer3dOrientation<T>::row2() const {
	return Vector3d<T>(matrix_[1][0], matrix_[1][1], matrix_[1][2]);
}

//! Accessor to the third row of the transformation matrix (read only)
/*!
 @return The third row of the transformation matrix
 */
template <class T>
Vector3d<T> Transformer3dOrientation<T>::row3() const {
	return Vector3d<T>(matrix_[2][0], matrix_[2][1], matrix_[2][2]);
}

//! Accessor to the given index of the transformation matrix (read only)
/*!
 @return The given index of the transformation matrix
 */
template <class T>
T Transformer3dOrientation<T>::matrix(Transformer3DOrientation::Index row, Transformer3DOrientation::Index column) const {
	return matrix_[row][column];
}

//! Accessor to the given index of the transformation matrix (read-write)
/*!
 @return A reference to the given index of the transformation matrix
 */
template <class T>
T& Transformer3dOrientation<T>::matrix(Transformer3DOrientation::Index row, Transformer3DOrientation::Index column) {
	return matrix_[row][column];
}

//! Transform the given orientation from the source space to the destination space
/*!
 @param from The coordinates of the orientation to transform in the source space
 @return The coordinates of the "from" orientation in the destination space
 */
template <class T>
Vector3d<T> Transformer3dOrientation<T>::transform(const Vector3d<T>& from) const {
	return Vector3d<T>(
		matrix_[0][0]*from.x() + matrix_[0][1]*from.y() + matrix_[0][2]*from.z(),
		matrix_[1][0]*from.x() + matrix_[1][1]*from.y() + matrix_[1][2]*from.z(),
		matrix_[2][0]*from.x() + matrix_[2][1]*from.y() + matrix_[2][2]*from.z()
	);
}

//! Create an inverse transformer, i.e., the transformer that converts orientations from the destination space to the source space
/*!
 @param ok If not nil, receives true if an inverse transformer exists and false otherwise
 @return The inverse transformer. If no such transformer exists, the identity transformer is returned
 */
template <class T>
Transformer3dOrientation<T> Transformer3dOrientation<T>::inverse(bool* ok) const {
	Vector3d<T> p1(1,0,0);
	Vector3d<T> p2(0,1,0);
	Vector3d<T> p3(0,0,1);
	Transformer3dOrientation res;
	bool b = res.set_transformation(transform(p1),transform(p2),transform(p3),p1,p2,p3);
	if ( ok != nil ) *ok = b;
	return res;
}

#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif

#endif
